#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

static void _warp_affine(const cv::Mat& src, const cv::Mat& M, cv::Mat& dst, bool bInvert) {
    dst.create(src.size(), src.type());
    cv::Mat MM;
    if (bInvert) {
        MM = M.clone();
    } else {
        cv::invertAffineTransform(M, MM);
    }
    const int BITS = 10;
    const int SCALE = 1 << BITS;
    int roundDelta = SCALE / 2;
    double m0 = MM.ptr<double>(0)[0];
    double m1 = MM.ptr<double>(0)[1];
    double m2 = MM.ptr<double>(0)[2];
    double m3 = MM.ptr<double>(1)[0];
    double m4 = MM.ptr<double>(1)[1];
    double m5 = MM.ptr<double>(1)[2];
    for (int i = 0; i < dst.rows; i++) {
        for (int j = 0; j < dst.cols; j++) {
            int src_x = cv::saturate_cast<int>(m0*j*SCALE);
            int src_y = cv::saturate_cast<int>(m3*j*SCALE);
            src_x += cv::saturate_cast<int>((m1*i+m2)*SCALE) + roundDelta;
            src_y += cv::saturate_cast<int>((m4*i+m5)*SCALE) + roundDelta;
            src_x >>= BITS;
            src_y >>= BITS;
            if (src_x >= 0 && src_x < src.cols && src_y >= 0 && src_y < src.rows) {
                dst.ptr<uchar>(i)[j] = src.ptr<uchar>(src_y)[src_x];
            } else {
                dst.ptr<uchar>(i)[j] = 0;
            }
        }
    }
}

int main(int argc, char **argv) {
    cv::Mat src = cv::imread("/home/xlll/Downloads/opencv/samples/data/lena.jpg", cv::IMREAD_GRAYSCALE);//cv::IMREAD_COLOR);
    if (src.empty()) {
        std::cout << "failed to read image" << std::endl;
        return EXIT_FAILURE;
    }
    
    cv::Point2f pt1[3], pt2[3];
    float xoffset = src.cols / 5;
    float yoffset = src.rows / 3;
    pt1[0] = cv::Point2f(0, 0);
    pt1[1] = cv::Point2f(xoffset, 0);
    pt1[2] = cv::Point2f(0, yoffset);
    pt2[0] = cv::Point2f(xoffset, yoffset);
    pt2[1] = cv::Point2f(xoffset*2, yoffset);
    pt2[2] = cv::Point2f(xoffset, yoffset*2);
    cv::Mat M;
    //M = cv::getAffineTransform(pt1, pt2);
    cv::Point2f center(0, 0);
    double angle = 30;
    double angle_radian = angle * CV_PI / 180;
    double scale = 1;
    //M = cv::getRotationMatrix2D(center, angle, scale);
    
    double scalex = 0.5;
    double scaley = 2;
    pt1[0] = cv::Point2f(0, 0);
    pt1[1] = cv::Point2f(xoffset, 0);
    pt1[2] = cv::Point2f(0, yoffset);
    pt2[0] = cv::Point2f(0, 0);
    pt2[1] = cv::Point2f(xoffset * scalex, 0);
    pt2[2] = cv::Point2f(0, yoffset * scaley);
    //M = cv::getAffineTransform(pt1, pt2);
    
    /*
    pt1[0] = cv::Point2f(0, 0);
    pt1[1] = cv::Point2f(xoffset, 0);
    pt1[2] = cv::Point2f(0, yoffset);
    pt2[0] = cv::Point2f(0, 0);
    //pt2[1] = cv::Point2f(xoffset, 0);
    //pt2[2] = cv::Point2f(yoffset*std::tan(angle_radian), yoffset);
    pt2[1] = cv::Point2f(xoffset, xoffset*std::tan(angle_radian));
    pt2[2] = cv::Point2f(0, yoffset);
    M = cv::getAffineTransform(pt1, pt2);
    
    cv::randu(M, 0, 1);
    cv::Mat M_inverse;
    cv::invertAffineTransform(M, M_inverse);
    std::cout << "M = " << M << std::endl;
    std::cout << "M_inverse = " << M_inverse << std::endl;
    
    cv::Mat MM_inverse(2, 3, CV_64FC1);
    double m0 = M.ptr<double>(0)[0];
    double m1 = M.ptr<double>(0)[1];
    double m2 = M.ptr<double>(0)[2];
    double m3 = M.ptr<double>(1)[0];
    double m4 = M.ptr<double>(1)[1];
    double m5 = M.ptr<double>(1)[2];
    double detM = m0*m4 - m1*m3;
    MM_inverse.ptr<double>(0)[0] = m4 / detM;
    MM_inverse.ptr<double>(0)[1] = -m1 / detM;
    MM_inverse.ptr<double>(0)[2] = (m1*m5 - m2*m4) / detM;
    MM_inverse.ptr<double>(1)[0] = -m3 / detM;
    MM_inverse.ptr<double>(1)[1] = m0 / detM;
    MM_inverse.ptr<double>(1)[2] = (m2*m3 - m0*m5) / detM;
    std::cout << "MM_inverse = " << MM_inverse << std::endl;
    
    cv::Mat M1 = cv::Mat::zeros(2, 3, CV_64FC1);
    //M1.ptr<double>(0)[0] = M1.ptr<double>(1)[1] = std::cos(angle_radian);
    //M1.ptr<double>(0)[1] = std::sin(angle_radian);
    //M1.ptr<double>(1)[0] = -std::sin(angle_radian);
    
    //M1.ptr<double>(0)[0] = scalex;
    //M1.ptr<double>(1)[1] = scaley;
    
    M1.ptr<double>(0)[0] = 1;
    //M1.ptr<double>(0)[1] = std::tan(angle_radian);
    M1.ptr<double>(1)[0] = std::tan(angle_radian);
    M1.ptr<double>(1)[1] = 1;
    //std::cout << "M1 = " << M1 << std::endl;
    
    center = cv::Point2f(src.cols / 2, src.rows / 2);
    angle = 30;
    scale = 0.5;
    M = cv::getRotationMatrix2D(center, angle, scale);
    
    angle_radian = angle * CV_PI / 180;
    double scale_cos = scale * std::cos(angle_radian);
    double scale_sin = scale * std::sin(angle_radian);
    M1.create(2, 3, CV_64FC1);
    M1.ptr<double>(0)[0] = scale_cos;
    M1.ptr<double>(0)[1] = scale_sin;
    M1.ptr<double>(0)[2] = (1-scale_cos)*center.x - scale_sin*center.y;
    M1.ptr<double>(1)[0] = -scale_sin;
    M1.ptr<double>(1)[1] = scale_cos;
    M1.ptr<double>(1)[2] = scale_sin*center.x + (1-scale_cos)*center.y;
    */
    
    float x1 = 0.f, y1 = 0.f;
    float x2 = src.cols - 10.f, y2 = 0.f;
    float x3 = 0.f, y3 = src.rows - 10.f;
    float x1p = 0.f, y1p = src.rows * 0.35f;
    float x2p = src.cols * 0.8f, y2p = src.rows * 0.2f;
    float x3p = src.cols * 0.15f, y3p = src.rows * 0.7f;
    pt1[0] = cv::Point2f(x1, y1);
    pt1[1] = cv::Point2f(x2, y2);
    pt1[2] = cv::Point2f(x3, y3);
    pt2[0] = cv::Point2f(x1p, y1p);
    pt2[1] = cv::Point2f(x2p, y2p);
    pt2[2] = cv::Point2f(x3p, y3p);
    M = cv::getAffineTransform(pt1, pt2);
    
    cv::Mat M1;
    M1.create(2, 3, CV_64FC1);
    double a = ((x3p - x2p) * (y1 - y2) - (y3 - y2) * (x2p - x1p)) / 
            ((y1 - y2) * (x3 - x2) + (y3 - y2) * (x1 - x2));
    double b = ((x3 - x2) * (x2p - x1p) + (x3p - x2p) * (x1 - x2)) / 
            ((x1 - x2) * (y3 - y2) + (x3 - x2) * (y1 - y2));
    double c = x1p - a * x1 - b* y1;
    double d = ((y3p - y2p) * (y1 - y2) - (y3 - y2) * (y2p - y1p)) / 
            ((y1 - y2) * (x3 - x2) + (y3 - y2) * (x1 - x2));
    double e = ((x3 - x2) * (y2p - y1p) + (y3p - y2p) * (x1 - x2)) / 
            ((x1 - x2) * (y3 - y2) + (x3 - x2) * (y1 - y2));
    double f = y1p - d * x1 - e* y1;
    M1.ptr<double>(0)[0] = a;
    M1.ptr<double>(0)[1] = b;
    M1.ptr<double>(0)[2] = c;
    M1.ptr<double>(1)[0] = d;
    M1.ptr<double>(1)[1] = e;
    M1.ptr<double>(1)[2] = f;
    
    std::cout << "M = " << M << std::endl;
    std::cout << "M1 = " << M1 << std::endl;
    
    cv::Mat dst;
    cv::warpAffine(src, dst, M, src.size(), cv::INTER_NEAREST);
    cv::Mat dst2;
    _warp_affine(src, M, dst2, false);
    
    std::vector<cv::Point> nonzeros;
    cv::Mat nonzerosMat = dst != dst2;
    cv::findNonZero(nonzerosMat, nonzeros);
    std::cout << "non zero size = " << nonzeros.size() << std::endl;
    
    cv::imshow("src", src);
    cv::imshow("dst", dst);
    cv::waitKey(0);
    
    return 0;
}
